import matplotlib.pyplot as plt
import numpy as np
import math

T = 10
delta = 0.1
XK = 1.0
k = list(range(1, int(T/ delta) + 1)) #采样次数
K = [0] * len(k)   #增益
xhat_prev = 0
x_measure = [0] * len(k)
x_hat = [0] * len(k)
PK1 = [0] * len(k)
PK2 = [0] * len(k)
wk = [0] * len(k)
t = [0] * len(k)
e1 = [0] * len(k)
#计算测量值
for i in range(0, len(k)):
    wk[i] = np.random.normal(loc=0.0, scale=1.0, size=None)
    x_measure[i] = XK + wk[i]
    t[i] = (k[i] - 1) * delta

#估计值
for i in range(0, len(k)):
    K[i] = 1 / k[i]
    x_hat[i] = xhat_prev + K[i] * (x_measure[i] - xhat_prev)
    xhat_prev = x_hat[i]
    PK1[i] = wk[i] / math.sqrt(k[i])
    PK2[i] = -PK1[i]
    e1[i] = x_hat[i] - XK

plt.xlabel("Time(Sec)")
plt.ylabel("xhat")
plt.plot(t,x_measure,'-o',c = 'r',label = 'measures')
plt.plot(t,x_hat,'--',label = 'hat of zero-order iterative least squares filter')
plt.axhline(y=XK, c="k",label = 'True of x',ls="--", lw=2)
plt.legend()
plt.show()

plt.xlabel("Time(Sec)")
#plt.plot(t,e,'-o',label = 'error between estimates and measures')
#plt.axhline(y=Xk-xhat, c="k",label = 'error between estimates and true',ls="--", lw=2)
plt.plot(t,e1,'-o',label = 'error between estimates and true')
plt.legend()
plt.show()

plt.xlabel('Time(Sec)')
plt.ylabel('Error in Estimate')
plt.plot(t, PK1, c="k",label = 'Theory1 of Error',ls="--", lw=2)
plt.plot(t, PK2, c="k",label = 'Theory2 of Error',ls="--", lw=2)
plt.plot(t,e1,'-o',label = 'Simulation', lw =2)
plt.legend()
plt.show()
